/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.propagation.numerical;

import ore.Frame;
import ore.PositionVelocity;
import ore.Transform;
import ore.errors.OrekitException;
import ore.errors.PropagationException;
import ore.orbits.EquinoctialOrbit;

import org.apache.commons.math.geometry.Vector3D;

import java.util.Arrays;

/** 
 * This class sums up the contribution of several forces into orbit
 * and mass derivatives.
 *
 * The aim of this class is to gather the contributions of various
 * perturbing forces expressed as accelerations into one set of
 * time-derivatives of {@link ore.orbits.EquinoctialOrbit} plus one
 * mass derivatives.  It implements Gauss equations for the
 * equinoctial parameters.
 * 
 * The state vector handled internally has the following form.
 *   <pre>
 *     y[0] = a
 *     y[1] = ex
 *     y[2] = ey
 *     y[3] = hx
 *     y[4] = hy
 *     y[5] = lv
 *     y[6] = mass
 *   </pre>
 * where the six firsts paramters stands for the equinoctial
 * parameters and the 7th for the mass (kg) at the current time.
 * 
 * The proper way to use this class is to have the object implementing
 * the FirstOrderDifferentialEquations interface do the following
 * calls each time the computeDerivatives method is called:
 * <ul>
 *   <li>
 *     Reinitialize the instance using the
 *     {@link #initDerivatives(double[], EquinoctialOrbit)} method;
 *   </li>
 *   <li>
 *     Pass the instance to each force model in turn so that they can
 *     put their own contributions using the various
 *     AddXxxAcceleration methods;
 *   </li>
 *   <li>
 *     Finalize the derivatives by adding the Kepler natural evolution
 *     contribution.
 *   </li>
 * </ul>
 * 
 * @see ore.orbits.EquinoctialOrbit
 * @see ore.propagation.numerical.NumericalPropagator
 * 
 * @author Luc Maisonobe
 * @author Fabien Maussion
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class TimeDerivativesEquations 
    extends Object
    implements java.io.Serializable
{

    /**
     * Stored parameters
     */
    private EquinoctialOrbit storedParameters;
    /**
     * Reference to the derivatives array to initialize.
     */
    private double[] storedYDot;
    /**
     * First vector of the (q, s, w) local orbital frame.
     */
    private Vector3D lofQ;
    /**
     * Second vector of the (q, s, w) local orbital frame.
     */
    private Vector3D lofS;
    /**
     * First vector of the (t, n, w) local orbital frame.
     */
    private Vector3D lofT;
    /**
     * Second vector of the (t, n, w) local orbital frame.
     */
    private Vector3D lofN;
    /**
     * Third vector of both the (q, s, w) and (t, n, w) local orbital frames.
     */
    private Vector3D lofW;
    /**
     * Multiplicative coefficients for the perturbing accelerations along lofQ.
     */
    private double aQ;
    private double exQ;
    private double eyQ;
    /**
     * Multiplicative coefficients for the perturbing accelerations along lofS.
     */
    private double aS;
    private double exS;
    private double eyS;
    /**
     * Multiplicative coefficients for the perturbing accelerations along lofT.
     */
    private double aT;
    private double exT;
    private double eyT;
    /**
     * Multiplicative coefficients for the perturbing accelerations along lofN.
     */
    private double exN;
    private double eyN;
    /**
     * Multiplicative coefficients for the perturbing accelerations along lofW.
     */
    private double eyW;
    private double exW;
    private double hxW;
    private double hyW;
    private double lvW;
    /**
     * Kepler evolution on true latitude argument. 
     */
    private double lvKepler;


    /** 
     * @param orbit current orbit parameters
     */
    protected TimeDerivativesEquations(final EquinoctialOrbit orbit) {
        super();
        this.storedParameters = orbit;
        this.lofQ = Vector3D.ZERO;
        this.lofS = Vector3D.ZERO;
        this.lofT = Vector3D.ZERO;
        this.lofN = Vector3D.ZERO;
        this.lofW = Vector3D.ZERO;
        this.updateOrbitalFrames();
    }


    /**
     * @return First vector of the (q, s, w) local orbital frame
     */
    public Vector3D getQ() {
        return this.lofQ;
    }
    /** 
     * @return Second vector of the (q, s, w) local orbital frame
     */
    public Vector3D getS() {
        return this.lofS;
    }
    /** 
     * @return first vector of the (t, n, w) local orbital frame
     */
    public Vector3D getT() {
        return this.lofT;
    }
    /** 
     * @return second vector of the (t, n, w) local orbital frame
     */
    public Vector3D getN() {
        return this.lofN;
    }
    /** 
     * @return Third vector of both the (q, s, w) and (t, n, w) local
     * orbital frames
     */
    public Vector3D getW() {
        return this.lofW;
    }
    /**
     * Add the contribution of an acceleration expressed in (t, n, w)
     * local orbital frame.
     * 
     * @param t acceleration along the T axis (m/s<sup>2</sup>)
     * @param n acceleration along the N axis (m/s<sup>2</sup>)
     * @param w acceleration along the W axis (m/s<sup>2</sup>)
     */
    public void addTNWAcceleration(final double t, final double n, final double w) {
        this.storedYDot[0] += (this.aT  * t);
        this.storedYDot[1] += (this.exT * t + this.exN * n + this.exW * w);
        this.storedYDot[2] += (this.eyT * t + this.eyN * n + this.eyW * w);
        this.storedYDot[3] += (this.hxW * w);
        this.storedYDot[4] += (this.hyW * w);
        this.storedYDot[5] += (this.lvW * w);
    }
    /**
     * Add the contribution of an acceleration expressed in (q, s, w)
     * local orbital frame.
     * 
     * @param q acceleration along the Q axis (m/s<sup>2</sup>)
     * @param s acceleration along the S axis (m/s<sup>2</sup>)
     * @param w acceleration along the W axis (m/s<sup>2</sup>)
     */
    public void addQSWAcceleration(final double q, final double s, final double w) {
        this.storedYDot[0] += (this.aQ  * q + this.aS  * s);
        this.storedYDot[1] += (this.exQ * q + this.exS * s + this.exW * w);
        this.storedYDot[2] += (this.eyQ * q + this.eyS * s + this.eyW * w);
        this.storedYDot[3] += (this.hxW * w);
        this.storedYDot[4] += (this.hyW * w);
        this.storedYDot[5] += (this.lvW * w);
    }
    /**
     * Add the contribution of an acceleration expressed in the
     * inertial frame.  It is important to make sure this acceleration
     * is defined in the same frame as the orbit.
     * 
     * @param x acceleration along the X axis (m/s<sup>2</sup>)
     * @param y acceleration along the Y axis (m/s<sup>2</sup>)
     * @param z acceleration along the Z axis (m/s<sup>2</sup>)
     */
    public void addXYZAcceleration(final double x, final double y, final double z) {
        double t = (x * this.lofT.getX() + y * this.lofT.getY() + z * this.lofT.getZ());
        double n = (x * this.lofN.getX() + y * this.lofN.getY() + z * this.lofN.getZ());
        double w = (x * this.lofW.getX() + y * this.lofW.getY() + z * this.lofW.getZ());
        this.addTNWAcceleration(t,n,w);
    }
    /**
     * Add the contribution of an acceleration expressed in some
     * inertial frame.
     * 
     * @param gamma acceleration vector (m/s<sup>2</sup>)
     * @param frame frame in which acceleration is defined (must be an inertial frame)
     * @exception OrekitException if frame transforms cannot be computed
     */
    public void addAcceleration(final Vector3D gamma, final Frame frame)
        throws OrekitException
    {
        Transform T = frame.getTransformTo(this.storedParameters.getFrame(),
                                           this.storedParameters.getDate());
        Vector3D gammInRefFrame = T.transformVector(gamma);

        double t = Vector3D.dotProduct(gammInRefFrame, this.lofT);
        double n = Vector3D.dotProduct(gammInRefFrame, this.lofN);
        double w = Vector3D.dotProduct(gammInRefFrame, this.lofW);
        this.addTNWAcceleration(t,n,w);
    }
    /**
     * Add the contribution of the flow rate (dm/dt).
     * 
     * @param q The flow rate, must be negative (dm/dt)
     * @exception IllegalArgumentException If flow-rate is positive
     */
    public void addMassDerivative(final double q) {
        if (q > 0) {
            throw OrekitException.createIllegalArgumentException(
                  "positive flow rate (q: {0})", q);
        }
        else
            this.storedYDot[6] += q;
    }
    /** 
     * Add the contribution of the Kepler evolution.
     * 
     * Since the Kepler evolution if the most important, it should be
     * added after all the other ones, in order to improve numerical
     * accuracy.
     */
    protected void addKeplerContribution() {
        this.storedYDot[5] += this.lvKepler;
    }
    /** 
     * Initialize all derivatives to zero.
     * 
     * @param yDot Reference to the array where to put the derivatives.
     * @param orbit Current orbit parameters
     * 
     * @exception PropagationException The orbit evolved out of range
     */
    protected void initDerivatives(final double[] yDot,
                                   final EquinoctialOrbit orbit)
        throws PropagationException
    {

        this.storedParameters = orbit;
        this.updateOrbitalFrames();
        /*
         * Store derivatives array reference
         */
        this.storedYDot = yDot;
        /*
         * Initialize derivatives to zero
         */
        java.util.Arrays.fill(storedYDot, 0.0);
        /*
         * Intermediate variables
         */
        final double ex  = storedParameters.getEquinoctialEx();
        final double ey  = storedParameters.getEquinoctialEy();
        final double ex2 = ex * ex;
        final double ey2 = ey * ey;
        final double e2  = ex2 + ey2;
        final double e   = Math.sqrt(e2);
        if (e >= 1) {
            throw new PropagationException(
                  "orbit becomes hyperbolic, unable to propagate it further (e: {0})",
                  e);
        }
        /*
         * Intermediate variables
         */
        final double oMe2        = (1 - e) * (1 + e);
        final double epsilon     = Math.sqrt(oMe2);
        final double a           = storedParameters.getA();
        final double na          = Math.sqrt(orbit.getMu() / a);
        final double n           = na / a;
        final double lv          = storedParameters.getLv();
        final double cLv         = Math.cos(lv);
        final double sLv         = Math.sin(lv);
        final double excLv       = ex * cLv;
        final double eysLv       = ey * sLv;
        final double excLvPeysLv = excLv + eysLv;
        final double ksi         = 1 + excLvPeysLv;
        final double nu          = ex * sLv - ey * cLv;
        final double sqrt        = Math.sqrt(ksi * ksi + nu * nu);
        final double oPksi       = 2 + excLvPeysLv;
        final double hx          = storedParameters.getHx();
        final double hy          = storedParameters.getHy();
        final double h2          = hx * hx  + hy * hy;
        final double oPh2        = 1 + h2;
        final double hxsLvMhycLv = hx * sLv - hy * cLv;

        final double epsilonOnNA        = epsilon / na;
        final double epsilonOnNAKsi     = epsilonOnNA / ksi;
        final double epsilonOnNAKsiSqrt = epsilonOnNAKsi / sqrt;
        final double tOnEpsilonN        = 2 / (n * epsilon);
        final double tEpsilonOnNASqrt   = 2 * epsilonOnNA / sqrt;
        final double epsilonOnNAKsit    = epsilonOnNA / (2 * ksi);

        /*
         * Kepler natural evolution
         */
        this.lvKepler = n * ksi * ksi / (oMe2 * epsilon);
        /*
         * Coefficients along T
         */
        this.aT  = tOnEpsilonN * sqrt;
        this.exT = tEpsilonOnNASqrt * (ex + cLv);
        this.eyT = tEpsilonOnNASqrt * (ey + sLv);
        /*
         * Coefficients along N
         */
        this.exN = -epsilonOnNAKsiSqrt * (2 * ey * ksi + oMe2 * sLv);
        this.eyN =  epsilonOnNAKsiSqrt * (2 * ex * ksi + oMe2 * cLv);
        /*
         * Coefficients along Q
         */
        this.aQ  =  tOnEpsilonN * nu;
        this.exQ =  epsilonOnNA * sLv;
        this.eyQ = -epsilonOnNA * cLv;
        /*
         * Coefficients along S
         */
        this.aS  = tOnEpsilonN * ksi;
        this.exS = epsilonOnNAKsi * (ex + oPksi * cLv);
        this.eyS = epsilonOnNAKsi * (ey + oPksi * sLv);
        /*
         * Coefficients along W
         */
        this.lvW =  epsilonOnNAKsi * hxsLvMhycLv;
        this.exW = -ey * lvW;
        this.eyW =  ex * lvW;
        this.hxW =  epsilonOnNAKsit * oPh2 * cLv;
        this.hyW =  epsilonOnNAKsit * oPh2 * sLv;
    }
    /**
     * 
     */
    private void updateOrbitalFrames() {

        final PositionVelocity pvCoordinates = this.storedParameters.getPositionVelocity();
        /*
         * Compute orbital plane normal vector
         */
        this.lofW = Vector3D.crossProduct(pvCoordinates.getPosition(), 
                                          pvCoordinates.getVelocity()).normalize();
        /*
         * Compute (q, s, w) local orbital frame
         */
        this.lofQ = pvCoordinates.getPosition().normalize();
        this.lofS = Vector3D.crossProduct(lofW, lofQ);
        /*
         * Compute (t, n, w) local orbital frame
         */
        this.lofT = pvCoordinates.getVelocity().normalize();
        this.lofN = Vector3D.crossProduct(lofW, lofT);
    }
}
